#include <Wire.h>
#include <Adafruit_ADS1X15.h>
#include <ESP32Servo.h>

Adafruit_ADS1115 ads;
Servo myservo;

const int servoPin = 4;
const int adsChannel = 0;

// 时间控制变量 - 缩短间隔到2ms（最快约500次/秒更新）
unsigned long previousMillis = 0;
const long interval = 2;  // 核心优化：从5ms缩短到2ms

// 记录上一次角度
int lastAngle = -1;

// 电位器实际范围（保持你的校准值）
const int adcMin = 0;
const int adcMax = 17750;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  if (!ads.begin()) {
    Serial.println("Failed to initialize ADS1115!");
    while (1);
  }
  
  // 舵机初始化 - 确保快速响应
  ESP32PWM::allocateTimer(0);
  myservo.setPeriodHertz(50);  // 标准50Hz不可改（舵机硬件决定）
  myservo.attach(servoPin, 500, 2400);  // 脉冲范围可根据舵机微调
  
  // 初始化时直接让舵机归位到初始角度
  int16_t initAdc = ads.readADC_SingleEnded(adsChannel);
  int initAngle = map(initAdc, adcMin, adcMax, 0, 180);
  myservo.write(initAngle);
  lastAngle = initAngle;
  
  Serial.println("高速响应模式启动！");
}

void loop() {
  unsigned long currentMillis = millis();
  
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    
    // 优化1：直接读取并转换，减少中间变量
    int targetAngle = map(ads.readADC_SingleEnded(adsChannel), adcMin, adcMax, 0, 180);
    targetAngle = constrain(targetAngle, 0, 180);
    
    // 优化2：角度变化哪怕1度也立即响应（取消阈值限制）
    if (targetAngle != lastAngle) {
      myservo.write(targetAngle);  // 直接发送最新角度，舵机全力转动
      lastAngle = targetAngle;
      
      // 优化3：减少串口打印频率（避免占用资源）
      static unsigned long printMillis = 0;
      if (millis() - printMillis >= 50) {  // 每50ms打印一次
        printMillis = millis();
        Serial.print("角度: ");
        Serial.println(targetAngle);
      }
    }
    
  }
  
  // 确保loop()内无其他耗时操作，保持快速循环
}
